#include "modules/MonitorModule/TopCapObsMonitor.h"
#include "data_struct/base/DataSet.h"
#include "lib/WrapFuncs.h"
#include "data_struct/geometry/Point.h"
#include "data_struct/robot/Pose.h"
#include "lib/Time.h"
#include "modules/AssistModule/ParamLoadModule.h"
#include "modules/CtrlModule/PassivePlanner/TopCapObsPlanner.h"

#define NONEXIST_OBS 0
#define STATIC_OBS 1
#define DYNAMIC_OBS 2

namespace behavior_controller
{
TopCapObsMonitor top_cap_obs_monitor;

TopCapObsMonitor::TopCapObsMonitor() {}

void TopCapObsMonitor::run()
{
    if (!init)
    {
        init = true;
        double len = 0.15 - 0.01;
        double side_len = len * 0.707106781;
        top_cap_obs_planner.robot_shape_new = contour_areas.robot_shape();
        top_cap_obs_planner.robot_shape_new.resize(8);
        top_cap_obs_planner.robot_shape_new.ModifyContour(0, PointF(len, 0));
        top_cap_obs_planner.robot_shape_new.ModifyContour(1, PointF(side_len, side_len));
        top_cap_obs_planner.robot_shape_new.ModifyContour(2, PointF(0, len));
        top_cap_obs_planner.robot_shape_new.ModifyContour(3, PointF(-side_len, side_len));
        top_cap_obs_planner.robot_shape_new.ModifyContour(4, PointF(-len, 0));
        top_cap_obs_planner.robot_shape_new.ModifyContour(5, PointF(-side_len, -side_len));
        top_cap_obs_planner.robot_shape_new.ModifyContour(6, PointF(0, -len));
        top_cap_obs_planner.robot_shape_new.ModifyContour(7, PointF(side_len, -side_len));
    }
    if (top_cap_obs_planner.finished) //执行上盖响应后不会再进入，直至finished为true
    {
        PointCloud<double> in_contour_points;
        int point_count = CheakTopCapObs(in_contour_points);

        if (point_count > 10)
        {
            int obs_type = IsStaticOrDynamic(in_contour_points);
            if (obs_type == STATIC_OBS)
            {
                //DataSet::ctrl_state = TOP_CAP_OBS_PLAN;
                //top_cap_obs_planner.finished= false; //dpj
                std::cout << "top_cap_obs  find!" << std::endl;
            } else
            {
                std::cout << "top_cap_obs find at statrt point!" << std::endl;
            }
            std::cout << "point_count= " << point_count << std::endl;
            DataSet::ctrl_state = TOP_CAP_OBS_PLAN;
            top_cap_obs_planner.obs_type = obs_type;
            top_cap_obs_planner.finished = false; //dpj
        }
    } else
    {
        DataSet::ctrl_state = TOP_CAP_OBS_PLAN;
    }
}

int TopCapObsMonitor::CheakTopCapObs(PointCloud<double> &in_contour_points)
{
    int point_count = 0;
    //获得的激光雷达数据为激光雷达坐标系下的数据，需要转化到机器人坐标系
    //获得的激光雷达数据为  激光雷达坐标系已经转化到机器人坐标系
    PointCloud<double> local_point_cloud;

    for (auto pt : DataSet::scan_point_cloud)
    {
        //PoseF local_point_pose = PoseF(pt, 0).ConvertOriginCur(lidar_pose);
        //PointF local_point(local_point_pose.GetPoint());
        PointF local_point(PoseF(pt, 0).GetPoint());
        local_point_cloud.emplace_back(local_point);

        if (top_cap_obs_planner.robot_shape_new.IsPointInShape(local_point))
        {
            in_contour_points.emplace_back(local_point);
        }
    }
    point_count = in_contour_points.size();
    //std::cout<< "point_count= "<< point_count<< std::endl;
    if (point_count < 1)
    {
        lase_scan_point = local_point_cloud;
    }
    return point_count;
}

//in_contour_points为机器人坐标系
int TopCapObsMonitor::IsStaticOrDynamic(PointCloud<double> &in_contour_points)
{
    int static_obs = 0;
    for (auto pt_1 : in_contour_points) //遍历轮廓中的点
    {
        for (auto pt_2 : lase_scan_point) //遍历最后一次scan点
        {
            if (Funcs::DistToPoint(PoseF(pt_1, 0), PoseF(pt_2, 0)) < 0.03) //两点距离小于3cm，为同一物体
            {
                static_obs++;
            }
        }
    }
    //std::cout<< "in_contour_points.size()= "<<  in_contour_points.size() << "  lase_scan_point.size()= " << lase_scan_point.size() << "  static_obs= " << static_obs << std::endl;
    int dynamic_obs = (int) in_contour_points.size() - static_obs;
    if (dynamic_obs >= static_obs)
        return DYNAMIC_OBS;
    else
        return STATIC_OBS;
}

} // namespace behavior_controller